#|;;;; Created on 2010-12-19 14:22:45
;;;;;;;; Search structures
(defstruct problem
  "This is the  problem struct to define a problem 
  :operators are the operators
  :init-state the initial state of the problem
  :goal-test the goal to test the node against
  :path-cost-fn the fuction that specifies the cost to each node"
  operators
  init-state
  goal-test
  path-cost-fn
  )
(defstruct node
  "a search tree node"
  path-cost
  parent
  operator 
  depth
  state
  )
;;;;;;;; Plan Structures
(defstruct plan
  "The plan Abstract data type"
  start-step
  finish-step
  steps
  causal-links
  orderings
  consistent
  )
(defstruct action
  "The action Abstract data type
   actions are intermediate steps in the plan"
  name
  preconditions
  effects
  )
(defstruct start-step
  "The start step of the plan"
  effects
  )
(defstruct finish-step
  "The Fisnish step of the plan"
  preconditions
  )
(defstruct causal-link
  "A causal link in the plan"
  cause-step
  effect-step
  sat-pre-cond
  )
(defun make-minimal-plan (start-effects finish-preconditions)
  "constructs a plan that has only the start and finish steps"
  (let ((plan (make-plan :start-step (make-start-step :effects start-effects) :finish-step (make-finish-step :preconditions finish-preconditions))))
    (setf (plan-steps plan) (cons (plan-start-step plan) (cons (plan-finish-step plan) ())))
    (setf (plan-consistent plan) t)
    plan)
  )
(defun solution? (plan)
  "Is plan a solution? The answer is 't' if there are no open preconditions in plan's steps, 'nil' otherwise"
  (let ((flag t))
    (if (and (plan-p plan) (plan-consistent plan))
        (dolist (i (plan-steps plan))
          (let ((pre-conds nil))
            (cond 
              ((finish-step-p i) (setf pre-conds (finish-step-preconditions i)))
              ((action-p i) (setf pre-conds (action-preconditions i)))
              (t (setf pre-conds nil))
              )
            (if pre-conds
                (if (not (eql (count-if #'(lambda (p) (eql i (causal-link-cause-step p))) (plan-causal-links plan)) (length pre-conds)))
                    (progn 
                      (setf flag nil)
                      (return flag))
                    )
                )
            )
          )
        nil
        )
    )
  )
(defun copy-ops (ops)
  (let ((new-ops nil)
        (new-prec nil)
        (new-eff nil)
        (new-name nil))
    (dolist (i ops)
      (setf new-prec (copy-list (action-preconditions i)))
      (setf new-eff (copy-list (action-effects i)))
      (setf new-name (action-name i))
      (setf new-ops (reverse (cons (make-action :name new-name :preconditions new-prec :effects new-eff) (reverse new-ops))))
      )
    new-ops
    )
  )
(defun av-ops (state operators-original)
  "returns available operators (links) to add to the plan or nil if no operators exist"
  ;(print "new entry")
  (if (and (plan-p state) (plan-consistent state))
      (progn
        (let ((steps (plan-steps state))
              (ops nil)
              (operators (copy-ops operators-original)))
          (dolist (i steps)
            (let ((pre-conds nil))
              (cond 
                ((finish-step-p i) (setf pre-conds (finish-step-preconditions i)))
                ((action-p i) (setf pre-conds (action-preconditions i)))
                (t (setf pre-conds nil))
                )
              (dolist (j pre-conds)
                (if (not (find-if #'(lambda (l)(and (eql i (causal-link-cause-step l)) (not (eql 'fail (unify j (causal-link-sat-pre-cond l)))))) (plan-causal-links state)))
                    (progn
                      (let ((flag nil)) 
                        (dolist (p (plan-steps state))
                          (if (not (eql p i))
                              (let ((effs nil)
                                    (subs nil))
                                ;(print p)
                                (cond
                                  ((start-step-p p) (setf effs (start-step-effects p)))
                                  ((action-p p)(setf effs (action-effects p)))
                                  (t nil)
                                  )
                                ;(print effs)
                                (dolist (q effs)
                                  ;(print "checked effects")
                                  (setf subs (unify q j))
                                  (if (not (eql 'fail subs))
                                      (progn
                                        (setf ops (cons (cons (cons p i) q) ops))
                                        (setf flag t)
                                        )
                                      )
                                  )
                                )
                              )
                          )
                        (if (not flag)
                            (dolist (k operators)
                              (let ((effs (action-effects k))
                                    (subs nil))
                                (dolist (l effs)
                                  (setf subs (unify l j))
                                  (if (not (eql 'fail subs))
                                      (progn
                                        (setf ops (cons (cons (cons k i) j) ops)))
                                      )
                                  )
                                )
                              )
                            )
                        )
                      )
                    )
                )	
              )
            )
          ops
          )
        )
      )
  )
(defun apply-op (state-original op)
  (if (and (plan-p state-original) (consp op))
      (let*
        ((state (copy-plan state-original))
         (unif-eff (find-if #'(lambda (e) (not (eql 'fail (unify e (cdr op))))) (if (action-p (caar op)) (action-effects (caar op)) (start-step-effects (caar op)))))
         (subst (unify unif-eff (cdr op)))
         (eff-step-pre (if (action-p (caar op)) (action-preconditions (caar op))))
         (eff-step-eff (if (action-p (caar op)) (action-effects (caar op)) (start-step-effects (caar op))))
         (cuz-step-pre (if (action-p (cdar op)) (action-preconditions (cdar op)) (finish-step-preconditions (cdar op))))
         (cuz-step-eff (if (action-p (cdar op)) (action-effects (cdar op)))))
        ;TO-DO apply unification substitution to the effects and pre-conditions of the cause state
        (if eff-step-pre (setf eff-step-pre (mapcar #'(lambda (e)(apply-subst e subst)) eff-step-pre)))
        (if eff-step-eff (setf eff-step-eff (mapcar #'(lambda (e)(apply-subst e subst)) eff-step-eff)))
        (if cuz-step-pre (setf cuz-step-pre (mapcar #'(lambda (e)(apply-subst e subst)) cuz-step-pre)))
        (if cuz-step-eff (setf cuz-step-eff (mapcar #'(lambda (e)(apply-subst e subst)) cuz-step-eff)))
        
        (setf (plan-causal-links state) (cons (make-causal-link :cause-step (cdar op)
                                                                :effect-step (caar op)
                                                                :sat-pre-cond (cdr op))
                                              (plan-causal-links state)))	
        (setf (plan-orderings state) (cons (car op) (plan-orderings state)))
        (if (not (find (caar op) (plan-steps state)))
            (progn
              (setf (plan-steps state) (cons (caar op) (plan-steps state)))
              (setf (plan-orderings state) (cons (cons (cons (plan-start-step state) (plan-finish-step state)) (caar op)) (plan-orderings state))))
            )
        ;;;;TO-DO Find and handle threats
        (let ((threats (get-threats state)))
          (dolist(i threats)  
            (if (can-demote? state (causal-link-effect-step (cdr i)) (car i))
                (setf (plan-orderings state) (cons (cons (car i) (causal-link-effect-step (cdr i))) (plan-orderings state)))
                (if (can-promote? state (causal-link-cause-step (cdr i)) (car i))
                    (setf (plan-orderings state) (cons (cons (causal-link-cause-step (cdr i)) (car i)) (plan-orderings state)))
                    (progn
                      (setf (plan-consistent state) nil)
                      (return state))
                    )
                )
            )
          )
        state
        )
      )
  )
(defun get-threats (plan)
  (if (plan-p plan)
      (let ((threats nil)
            (threat-links nil))
        (dolist (i (plan-steps plan))
          (if (action-p i)
              (progn
                (setf threat-links 
                      (remove nil 
                              (mapcar #'(lambda (e)
                                          (if (eql 'not (car e))
                                              (let ((t-link (find-if #'(lambda(l) (and (equal (causal-link-sat-pre-cond l) (cadr e))
                                                                                       (not (equal i (causal-link-cause-step l)))
                                                                                       (not (or (find (cons i (causal-link-effect-step l)) (plan-orderings plan) :test #'equal)
                                                                                                (find (cons (causal-link-cause-step l) i) (plan-orderings plan) :test #'equal))))) 
                                                                     (plan-causal-links plan) )))
                                                t-link)))
                                      (copy-list (action-effects i)))))
                (dolist (j threat-links)
                  (setf threats (cons (cons i j) threats)))
                )
              )
          )
        threats
        )
      )
  )
(defun can-demote? (plan effect-step s-threat)
  (cond
    ((start-step-p effect-step)nil)
    ((find (cons effect-step s-threat) (plan-orderings plan) :test #'eql)nil)
    (t t)
    )
  )
(defun can-promote? (plan cause-step s-threat)
  (cond
    ((finish-step-p cause-step)nil)
    ((find (cons s-threat cause-step) (plan-orderings plan) :test #'eql) nil)
    (t t))
  )
(defun get-open-conditions (plan)
  (let ((op-cond nil))
    (if (plan-p plan)
        (dolist (i (plan-steps plan))
          (setf op-cond (append op-cond (remove nil (mapcar #'(lambda (conds) 
                                                                (mapcar #'(lambda (c) 
                                                                            (if (not (find c 
                                                                                           (plan-causal-links plan) 
                                                                                           :key #'causal-link-sat-pre-cond)) 
                                                                                c)) 
                                                                        conds))
                                                            (cond 
                                                              ((action-p i) (action-preconditions i)) 
                                                              ((finish-step-p i) (finish-step-preconditions i)))))))
          )
        )
    op-cond
    )
  ) 
(defun search-heuristic (node problem)
  (if (and (node-p node) (problem-p problem))
      (let* ((plan  (node-state node))
             (ops (copy-ops (problem-operators problem)))
             (open-conds (get-open-conditions plan))
             (min-op nil))
        (dolist (i ops)
          (setf (action-effects i) (remove-if #'(lambda(e)
                                                  (eql 'not (car e)))  
                                              (action-effects i)))
          )
        (loop do
              (if (eql 0 (length open-conds))
                  (progn
                    (print (length min-op))
                    (return (length min-op)))
                  )
              (let ((max-sat-precond nil)
                    (curr-sat-precond-n nil)
                    (curr-sat-precond nil)
                    (max-act nil))
                (dolist (i ops)
                  ;(print "hi")
                  (setf curr-sat-precond-n (mapcar #'(lambda (e) (mapcar #'(lambda (p) (if (not (eql 'fail (unify e p))) p)) open-conds)) (action-effects i)))
                  ;(print "end hi")
                  (dolist(j curr-sat-precond-n)
                    (setf curr-sat-precond (append j curr-sat-precond))
                    )
                  (setf curr-sat-precond (remove nil curr-sat-precond))
                  (if (> (length curr-sat-precond) (length max-sat-precond))
                      (progn
                        (setf max-sat-precond curr-sat-precond)
                        (setf max-act i)
                        )
                      )
                  )
                (dolist (k max-sat-precond)
                  (setf open-conds (remove k open-conds))
                  )
                (setf ops (remove max-act ops))
                )
              ) 
        )
      )
  )|#